#include "ObstacleManage.h"
#include "AiBox_LOG.h"
#include "OMPLPlanner.h"
using namespace Planner;
#define VALID_TIMESTAMP_MS 5*1000 //5S
#define COLLISION_DISTANCE 6.0f //m
std::shared_ptr<ObstacleManage>  ObstacleManage::_singleton=nullptr;
std::shared_ptr<ObstacleManage> ObstacleManage::getInstance()
{
    if(_singleton.get()==nullptr){
        new ObstacleManage();
    }
    return _singleton;
}
ObstacleManage::ObstacleManage()
{
    _singleton=std::shared_ptr<ObstacleManage>(this);
    _obs_thread=std::make_shared<std::thread>(&ObstacleManage::obstacleManageThread,this);
    if(_obs_thread.get()==nullptr){
        AIBOX_LOGE("Creat a Obstacle manage thread err.");
    }
}
void 
ObstacleManage::obstacleManageThread()
{
    while(true){
        if(_obstacle_map.empty()){
            std::this_thread::sleep_for(std::chrono::seconds(20));
            continue;
        }
        {
            std::lock_guard<std::mutex>slok(_thread_mutex);
            //handle the old message
            std::map<int,LocationInfo>::iterator ite=_obstacle_map.begin();
            //erase the old location info
            while(ite!=_obstacle_map.end()){
                int64_t dlt_ms=CURRENT_TIME-ite->second.getTimestamp().count();
                if(dlt_ms>VALID_TIMESTAMP_MS){
                    ite=_obstacle_map.erase(ite);
                }else{
                    ite++;
                }
            }
            //handle the collision
            for(const auto& item:_obstacle_map){
                for(const auto& obs_item:_obstacle_map){
                    if(item.first==obs_item.first){
                        continue;
                    }
                    if(item.second.distanceTo(obs_item.second)<COLLISION_DISTANCE){
                        //handle collision risk
                        auto target_item=_target_map.find(item.first);
                        if(target_item!=_target_map.end()&&item.second.distanceTo(target_item->second)>2.0f){
                            new OMPLPlanner(item.first,item.second,target_item->second,_obstacle_map);
                        }
                    }
                }
            }
        }
        std::this_thread::sleep_for(std::chrono::seconds(20));
    }
}

void 
ObstacleManage::addObstacle(int id,const LocationInfo& temp)
{
    //update the obstacle map
    std::lock_guard<std::mutex>slok(_thread_mutex);
    auto item=_obstacle_map.find(id);
    if(item!=_obstacle_map.end()){
        item->second=temp;
    }else{
        _obstacle_map.insert(std::make_pair(id,temp));
    }
}
void 
ObstacleManage::addTarget(int id,const LocationInfo& temp)
{
    auto item=_target_map.find(id);
    if(item!=_target_map.end()){
        item->second=temp;
    }else{
        _target_map.insert(std::make_pair(id,temp));
    }
}